import traci
from geopy.distance import geodesic
import time
import redis
import math

pool = redis.ConnectionPool(host='127.0.0.1', port=6379, db=0)
r = redis.StrictRedis(connection_pool=pool)


def transformlat(lng, lat):
    PI = 3.1415926535897932384626
    ret = -100.0 + 2.0 * lng + 3.0 * lat + 0.2 * lat * \
          lat + 0.1 * lng * lat + 0.2 * math.sqrt(abs(lng))
    ret += (20.0 * math.sin(6.0 * lng * PI) + 20.0 *
            math.sin(2.0 * lng * PI)) * 2.0 / 3.0
    ret += (20.0 * math.sin(lat * PI) + 40.0 *
            math.sin(lat / 3.0 * PI)) * 2.0 / 3.0
    ret += (160.0 * math.sin(lat / 12.0 * PI) + 320 *
            math.sin(lat * PI / 30.0)) * 2.0 / 3.0
    return ret


def transformlng(lng, lat):
    PI = 3.1415926535897932384626
    ret = 300.0 + lng + 2.0 * lat + 0.1 * lng * lng + \
          0.1 * lng * lat + 0.1 * math.sqrt(abs(lng))
    ret += (20.0 * math.sin(6.0 * lng * PI) + 20.0 *
            math.sin(2.0 * lng * PI)) * 2.0 / 3.0
    ret += (20.0 * math.sin(lng * PI) + 40.0 *
            math.sin(lng / 3.0 * PI)) * 2.0 / 3.0
    ret += (150.0 * math.sin(lng / 12.0 * PI) + 300.0 *
            math.sin(lng / 30.0 * PI)) * 2.0 / 3.0
    return ret


def wgs84togcj02(lng, lat):
    PI = 3.1415926535897932384626
    ee = 0.00669342162296594323
    a = 6378245.0
    dlat = transformlat(lng - 105.0, lat - 35.0)
    dlng = transformlng(lng - 105.0, lat - 35.0)
    radlat = lat / 180.0 * PI
    magic = math.sin(radlat)
    magic = 1 - ee * magic * magic
    sqrtmagic = math.sqrt(magic)
    dlat = (dlat * 180.0) / ((a * (1 - ee)) / (magic * sqrtmagic) * PI)
    dlng = (dlng * 180.0) / (a / sqrtmagic * math.cos(radlat) * PI)
    mglat = lat + dlat
    mglng = lng + dlng
    return mglng, mglat


def outPut(vehicleId):
    x, y = traci.vehicle.getPosition(vehicleId)
    lng, lat = traci.simulation.convertGeo(x, y)
    lng, lat = wgs84togcj02(lng, lat)
    angle = traci.vehicle.getAngle(vehicleId)
    return lng, lat, angle


def outPutPerson(personId):
    x, y = traci.person.getPosition(personId)
    lng, lat = traci.simulation.convertGeo(x, y)
    lng, lat = wgs84togcj02(lng, lat)
    angle = traci.person.getAngle(personId)
    return lng, lat, angle


out_timeStamp = 0


def forwardCollision(mainVehicle, otherVehicle):
    global out_timeStamp
    mainLng, mainLat, angle = outPut(mainVehicle)
    otherLng, otherLat, angle = outPut(otherVehicle)
    dis = getDistance(mainLat, mainLng, otherLat, otherLng)
    main_speed = traci.vehicle.getSpeed(mainVehicle)
    other_speed = traci.vehicle.getSpeed(otherVehicle)
    if other_speed == 0.0:
        return
    for veh_id in traci.vehicle.getIDList():
        if 'FH1102' not in veh_id:
            return
        else:
            timer = dis / abs(main_speed - other_speed)
            otherLane = traci.vehicle.getLaneID(otherVehicle)
            mainLane = traci.vehicle.getLaneID(mainVehicle)
            traci.vehicle.setSpeed(otherVehicle, 5.0)
            traci.vehicle.changeLane(otherVehicle, 1, 20.0)
            traci.vehicle.changeLane(mainVehicle, 1, 20.0)
            current_time = time.time()
            if timer < 3 and mainLane == otherLane and current_time - out_timeStamp >= 1:
                out_timeStamp = current_time
                traci.vehicle.slowDown(mainVehicle, 5, 5.0)
                print("前向碰撞")
        # traci.vehicle.changeLaneRelative(mainVehicle, 1, 5.0)
        # traci.vehicle.setSpeed(otherVehicle, -1)


def innormalVeh(mainVehicle):
    global out_timeStamp
    current_time = time.time()
    edgeId = traci.lane.getEdgeID(traci.vehicle.getLaneID(mainVehicle))
    mainLng, mainLat, angle = outPut(mainVehicle)
    position = traci.vehicle.getLanePosition(mainVehicle)
    if edgeId == '1046113965#0' and position > 40.0:
        res = r.setnx("innormalVeh", 'innormalVeh')
        if res is True:
            traci.vehicle.add('FH2001', '2001')
            r.expire("innormalVeh", 10)
    for veh_id in traci.vehicle.getIDList():
        if 'FH2001' not in veh_id:
            continue
        otherLng, otherLat, angle = outPut('FH2001')
        traci.vehicle.setSpeed('FH2001', 5.0)
        if math.isinf(otherLng) or math.isinf(otherLat):
            return
        dis = getDistance(mainLat, mainLng, otherLat, otherLng)
        if dis < 10.0 and current_time - out_timeStamp >= 1:
            out_timeStamp = current_time
            print("异常")


def personEvent(mainVehicle):
    edgeId = traci.lane.getEdgeID(traci.vehicle.getLaneID(mainVehicle))
    mainLng, mainLat, angle = outPut(mainVehicle)
    position = traci.vehicle.getLanePosition(mainVehicle)
    # print(traci.vehicle.getAcceleration(mainVehicle))
    # if edgeId == '1046113965#3':
    #     res = r.setnx("person", 'person')
    #     if res is True:
    #         # routeId = 'personRou'
    #         # traci.route.add(routeId, ['1046113965#3', '398016354#5'])
    #         print(111111111)
    #         traci.person.add('persons', '1046113965#3', 200.0)
    #         print(2222222222)
    #         r.expire("person", 20)


def getDistance(lat1, lng1, lat2, lng2):
    dis = geodesic((lat1, lng1), (lat2, lng2)).m  #
    return dis
